/*
 * 
 * Author: Chao Wang    Email:chw412@lehigh.edu
 * 
 * Decription:
 * 
 * Each agent is designed according to a finite state machine that has several stages : 
 * INTENT, CARRY, EXPLORE, PICKUP, DROPOFF, IDLE, ORDER and UNORDER.
 * 
 * Each agent starts with IDLE, then it tries to determine a package to go for and broadcasts to
 * others that it has ordered that package. After that, the agent enters into a stage called "INTENT"
 * and go to the package according to the path found by A* algorithm. After a successful pickup, the 
 * agents enters into CARRY mode and walk to the package's delivery point(i.e., destination). After
 * The agent feels that it can drop the package, it will enters into DROPOFF mode and drop the package.
 * Unsuccessfull delivery lead an agent to broadcast to others the package it fails to deliver, allowing
 * other agents to delivery again.
 * 
 * 
 */

package chw412;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.Queue;

import chw412.KnownAgent.agent_state;

import com.sun.xml.internal.bind.v2.runtime.unmarshaller.XsiNilLoader.Array;

import agent.Action;
import agent.Percept;
import agent.Agent;
import pacworld.*;

import java.util.*;

import agent.Action;

//this is the state of the map as far as I known.
enum map_state {
	Package, Clear, Unknown
};

/**
 * An abstract software agent class. The agent must be managed by the
 * Environment class, which calls its see() and selectAction() methods.
 */
public class PacAgent extends Agent {

	protected String id;
	protected int x = -1;
	protected int y = -1;

	protected Action next_act = null;

	protected PacPercept percept = null;

	// Other agents I know
	protected ArrayList<KnownAgent> agt_info = null;

	// Packages I know
	protected ArrayList<KnownPackage> pkg_info = null;

	// the map representing the world.
	protected map_state[][] map = null;
	protected int map_size = -1;

	// my current goal
	protected Location goal = null;

	// my current delivery destination, if any
	protected Location dest = null;

	// this is the plan to get close to my goal.
	protected ArrayList<Integer> plan = null;

	// move history records the direction that I move in each step.
	protected ArrayList<Integer> move_history = null;

	// the last action executed
	protected Action last_act = null;

	// the location of the package dropped in the last turn, if last turn is not Drop-off
	// action, pkg_delivered == -1
	protected int pkg_delivered = -1;

	protected KnownPackage ordered_package = null;
	// the package picked up in the last turn, if last turn is not picked-up
	// action, pkg_pickedup = -1
	protected int pkg_pickedup = -1;

	//if general pickup and dropoff process fails unexpectedly. we select n-nearest goal.
	protected int easiest_level = 1;
	
	//counter of deadlock
	protected int deadlock_count = 0;
	
	//the deadlock count limit
	static final int deadlock_count_limit = 3;
	
	//current position
	Location cur = null;
	
	public enum state_t {
		INTENT, // the agent is pursuing some certain target package
		CARRY, // the agent is carrying some package
		EXPLORE, // the agent is exploring the unknown area in the world
		PICKUP, DROPOFF, IDLE, // , by default, agents are idling.
		ORDER,
		UNORDER
	};

	private state_t state;
	
	//the object type at each location
	public static enum obj_t {
		ME,
		OTHER_AGENT,
		UNHELD_PACKAGE,
		OTHERS_HELD_PACKAGE,
		MY_HELD_PACKAGE
	};

	/**
	 * All subclass should create a one-arg constructor that takes an integer
	 * and invokes this constructor. This constructor would be used in
	 * multi-agent environments in order to distinguish the different agents.
	 */
	public PacAgent(int id) {
		this.id = Integer.toString(id);
		agt_info = new ArrayList<KnownAgent>();
		pkg_info = new ArrayList<KnownPackage>();
		move_history = new ArrayList<Integer>();
		plan = new ArrayList<Integer>();
		state = state_t.IDLE;
	}

	/**
	 * update the map information according to the location of agents I know
	 * @param ka
	 *            the agent that just joined our team
	 */
	void update_map(KnownAgent ka) {
		int ax = ka.getX();
		int ay = ka.getY();
		for (int i = ax - PacPercept.VIS_RADIUS; i <= ax
				+ PacPercept.VIS_RADIUS; ++i) {
			for (int j = ay - PacPercept.VIS_RADIUS; j <= ay
					+ PacPercept.VIS_RADIUS; ++j) {
				if (in_map_range(new Point(i, j, null))
						&& map[i][j] == map_state.Unknown) {
					map[i][j] = map_state.Clear;
				}
			}
		}
	}

	/**
	 * Parse the received messages at each turn
	 */
	boolean parse_messages(String[] msgs) {
		int pid, x, y, z, w;
		String h, o;
		KnownPackage kp;
		
		String idd;
		
		for (String str : msgs) {
			switch (str.charAt(0)) {
			case 'P': // messages that indicate that new packages identified by
						// some team member.
				String pkgs = str.substring(0, str.indexOf('A'));
				String agt = str.substring(str.indexOf('A'), str.length());
				
				idd = agt.substring(agt.indexOf('A') + 1, agt.indexOf('X'));
				
				if(Integer.parseInt(idd) == Integer.parseInt(id))
					break;
				
				x = Integer.parseInt(agt.substring(agt.indexOf('X') + 1, agt.indexOf('Y')));
				y = Integer.parseInt(agt.substring(agt.indexOf('Y') + 1, agt.length()));
				update_map(new KnownAgent(idd, x, y));
				
				// parse package info
				String[] pkg_strs = pkgs.split("P");
				for (String pkg_str : pkg_strs) {
					if (pkg_str.length() > 0) {
						pid = Integer.parseInt(pkg_str.substring(0,
								pkg_str.indexOf('X')));
						x = Integer
								.parseInt(pkg_str.substring(
										pkg_str.indexOf('X') + 1,
										pkg_str.indexOf('Y')));
						y = Integer
								.parseInt(pkg_str.substring(
										pkg_str.indexOf('Y') + 1,
										pkg_str.indexOf('Z')));
						z = Integer
								.parseInt(pkg_str.substring(
										pkg_str.indexOf('Z') + 1,
										pkg_str.indexOf('W')));
						w = Integer
								.parseInt(pkg_str.substring(
										pkg_str.indexOf('W') + 1,
										pkg_str.indexOf('H')));
						h = pkg_str.substring(pkg_str.indexOf('H') + 1,
								pkg_str.indexOf('O'));
						o = pkg_str.substring(pkg_str.indexOf('O') + 1,
								pkg_str.length());
						
						kp = new KnownPackage(pid, x, y, z ,w, 
								((h.equals("Y"))?true:false), 
								((o.equals("Y"))?true:false));
						
						if(map[x][y] == map_state.Package){
							;
						} else if (map[x][y] == map_state.Unknown) {
							pkg_info.add(kp);
							map[x][y] = map_state.Package;
						} else if(map[x][y] == map_state.Clear){
							pkg_info.add(kp);
							map[x][y] = map_state.Package;
						}
						
						if(kp.held == true && kp.ordered == true)	//this is a pickup message
								map[x][y] = map_state.Clear;
						else if (kp.held == false && kp.ordered == true){	//this is an order message
								pkg_info.remove(kp);
								map[x][y] = map_state.Package;
						}
						else if (kp.held == false && kp.ordered == false){	//this is a package detection message or unorder message
								;
						}
					}
				}
				
				break;
			}				
	}
	return true;
}
	
	/*
	 * calculate the distance between to locations
	 */
	int dist(Location l1, Location l2) {
		return Math.abs(l1.getX() - l2.getX())
				+ Math.abs(l1.getY() - l2.getY());
	}
	
	
	/*
	 * look for a package by its location in my package account
	 */
	KnownPackage get_package_at_loc(Location l) {
		for (int i = 0; i < pkg_info.size(); ++i) {
			if (pkg_info.get(i).getX() == l.getX()
					&& pkg_info.get(i).getY() == l.getY())
				return pkg_info.get(i);
		}
		return null;
	}
	
	/*
	 * look for a package by its id in my package account
	 */
	KnownPackage get_package_info(int id) {
		for (int i = 0; i < pkg_info.size(); ++i) {
			if (pkg_info.get(i).getId() == id)
				return pkg_info.get(i);
		}
		return null;
	}
	
	/*
	 * get the direction between two adjacent locations, from l1 to l2 If l1 and
	 * l2 are not adjacent, return -1
	 */
	int get_direction(Location l1, Location l2) {
		int delta_x = l2.getX() - l1.getX();
		int delta_y = l2.getY() - l1.getY();
		if (delta_x == 0 && delta_y == -1)
			return Direction.NORTH;
		else if (delta_x == 1 && delta_y == 0)
			return Direction.EAST;
		else if (delta_x == 0 && delta_y == 1)
			return Direction.SOUTH;
		else if (delta_x == -1 && delta_y == 0)
			return Direction.WEST;
		else
			return -1;
	}

	/*
	 * check if I can move according to the direction I am going to move, the
	 * direction to which I am holding my package, if any, and my current
	 * instant percept.
	 */

	boolean check_movable(Integer dir, Integer pkg_dir) {
		VisibleAgent[] vas = percept.getVisAgents();
		VisiblePackage[] vps = percept.getVisPackages();

		if (pkg_dir == -1) { // I do not hold any package
			Location mf = new Location(this.x + Direction.DELTA_X[dir], this.y
					+ Direction.DELTA_Y[dir]);
			if (!in_map_range(new Point(mf, null)))
				return false;
			for (VisibleAgent va : vas) {
				if (mf.getX() == va.getX() && mf.getY() == va.getY())
					return false;
			}
			for (VisiblePackage vp : vps) {
				if (mf.getX() == vp.getX() && mf.getY() == vp.getY())
					return false;
			}
			return true;
		} else { // I am holding a package in direction pkg_dir
			if (dir == (pkg_dir + 1) % 4 || dir == (pkg_dir + 3) % 4) {
				Location mf = new Location(this.x + Direction.DELTA_X[dir],
						this.y + Direction.DELTA_Y[dir]);
				Location pf = new Location(this.x + Direction.DELTA_X[pkg_dir]
						+ Direction.DELTA_X[dir], this.y
						+ Direction.DELTA_Y[pkg_dir] + Direction.DELTA_Y[dir]);
				if (!in_map_range(new Point(mf, null))
						|| !in_map_range(new Point(pf, null)))
					return false;
				for (VisibleAgent va : vas) {
					if (mf.getX() == va.getX() && mf.getY() == va.getY()
							|| pf.getX() == va.getX() && pf.getY() == va.getY())
						return false;
				}
				for (VisiblePackage vp : vps) {
					if (mf.getX() == vp.getX() && mf.getY() == vp.getY()
							|| pf.getX() == vp.getX() && pf.getY() == vp.getY())
						return false;
				}
				return true;
			} else if (dir == (pkg_dir + 2) % 4) {
				Location mf = new Location(this.x + Direction.DELTA_X[dir],
						this.y + Direction.DELTA_Y[dir]);
				if (!in_map_range(new Point(mf, null)))
					return false;
				for (VisibleAgent va : vas) {
					if (mf.getX() == va.getX() && mf.getY() == va.getY())
						return false;
				}
				for (VisiblePackage vp : vps) {
					if (mf.getX() == vp.getX() && mf.getY() == vp.getY())
						return false;
				}
				return true;
			} else if (dir == pkg_dir) {
				Location pf = new Location(this.x + 2 * Direction.DELTA_X[dir],
						this.y + 2 * Direction.DELTA_Y[dir]);
				if (!in_map_range(new Point(pf, null)))
					return false;
				for (VisibleAgent va : vas) {
					if (pf.getX() == va.getX() && pf.getY() == va.getY())
						return false;
				}
				for (VisiblePackage vp : vps) {
					if (pf.getX() == vp.getX() && pf.getY() == vp.getY())
						return false;
				}
				return true;
			} else {
				assert (false);
				return true;
			}
		}
	}

	/*
	 * calculate the path from me to point p. Given that I am holding a package
	 * on my dir side or -1 if none. If no such path exist, return null
	 */
	ArrayList<Integer> compute_path(Point p, int pkg_dir) {
		Astar astar = new Astar(map, p, new Point(this.x, this.y, null),
				pkg_dir);
		if (astar.astar_search()) { // we found a path from me to target point
			return astar.construct_path();
		}
		return null;
	}

	/*
	 * find a reachable destination that is closest to location dest. pkg_dir
	 * indicates the direction I am holding the package. If no package is held,
	 * pkg_dir = -1 return null if none exist.
	 */
	Location Find_Nearest_Unknown(Location cur) {
		boolean[][] visited = new boolean[map_size][map_size];
		for (int i = 0; i < map_size; ++i)
			for (int j = 0; j < map_size; ++j)
				visited[i][j] = false;

		Queue<Location> v1 = new LinkedList<Location>();
		v1.add(cur);
		visited[cur.getX()][cur.getY()] = true;

		while (!v1.isEmpty()) {
			Location t = v1.remove();
			if (map[t.getX()][t.getY()] == map_state.Unknown) {
				return t;
			}

			for (int dir = 0; dir <= 3; ++dir) {
				Location p = new Location(t.getX() + Direction.DELTA_X[dir],
						t.getY() + Direction.DELTA_Y[dir]);
				if (in_map_range(new Point(p, null))
						&& visited[p.getX()][p.getY()] == false) {
					v1.add(p);
					visited[p.getX()][p.getY()] = true;
				}
			}
		}

		return null;
	}

	/*
	 * find the nearest clear location from location dest
	 */
	Location Find_Nearest_Clear(Location dest) {
		boolean[][] visited = new boolean[map_size][map_size];
		for (int i = 0; i < map_size; ++i)
			for (int j = 0; j < map_size; ++j)
				visited[i][j] = false;

		Queue<Location> v1 = new LinkedList<Location>();
		v1.add(dest);
		visited[dest.getX()][dest.getY()] = true;

		while (!v1.isEmpty()) {
			Location t = v1.remove();
			if (map[t.getX()][t.getY()] == map_state.Clear) {
				return t;
			}

			for (int dir = 0; dir <= 3; ++dir) {
				Location p = new Location(t.getX() + Direction.DELTA_X[dir],
						t.getY() + Direction.DELTA_Y[dir]);
				if (in_map_range(new Point(p, null))
						&& visited[p.getX()][p.getY()] == false) {
					v1.add(p);
					visited[p.getX()][p.getY()] = true;
				}
			}
		}

		return null;
	}

	/*
	 * find the nearest package from location cur
	 */
	
	Location Find_Nearest_Package(Location cur) {
		boolean[][] visited = new boolean[map_size][map_size];
		for (int i = 0; i < map_size; ++i)
			for (int j = 0; j < map_size; ++j)
				visited[i][j] = false;

		Queue<Location> v1 = new LinkedList<Location>();
		v1.add(cur);
		visited[cur.getX()][cur.getY()] = true;

		while (!v1.isEmpty()) {
			Location t = v1.remove();
			if (map[t.getX()][t.getY()] == map_state.Package) {
				return t;
			}

			for (int dir = 0; dir <= 3; ++dir) {
				Location p = new Location(t.getX() + Direction.DELTA_X[dir],
						t.getY() + Direction.DELTA_Y[dir]);
				if (in_map_range(new Point(p, null))
						&& visited[p.getX()][p.getY()] == false) {
					v1.add(p);
					visited[p.getX()][p.getY()] = true;
				}
			}
		}

		return null;
	}
	
	/*
	 * find the nearest package from location cur
	 * 
	 */
	Location Find_Easiest_Package_Known(Location cur){
		KnownPackage easiest = null;
		int min = Integer.MAX_VALUE;
		for(KnownPackage kp : pkg_info){
			int d = dist(new Location(kp.getX(), kp.getY()), cur) + dist(new Location(kp.getX(), kp.getY()), new Location(kp.getDestX(), kp.getDestY()));
			if (d < min){
				min = d;
				easiest = kp;
			}
		}
		if(easiest != null)
			return new Location(easiest.getX(), easiest.getY());
		else
			return null;
	}

	/*
	 * find the nth-nearest package from location cur to allow agents 
	 * find different packages, not only the nearest one from cur.
	 * 
	 */
	Location Find_N_Easiest_Package_Known(Location cur, int easiest_level){
		ArrayList<KnownPackage> another_pkg_info = new ArrayList<KnownPackage>();
		for(KnownPackage kp : pkg_info) {
			another_pkg_info.add(kp);
		}
		
		KnownPackage n_easiest = null;
		while(true){
			int min = Integer.MAX_VALUE;
			for(KnownPackage kp : another_pkg_info){
				int d = dist(new Location(kp.getX(), kp.getY()), cur) + dist(new Location(kp.getX(), kp.getY()), new Location(kp.getDestX(), kp.getDestY()));
				if (d < min){
					min = d;
					n_easiest = kp;
				}
			}
			
			easiest_level--;
			if(easiest_level == 0){
				break;
			}
			
			another_pkg_info.remove(n_easiest);
		}
		
		if(n_easiest != null)
			return new Location(n_easiest.getX(), n_easiest.getY());
		else
			return null;
	}
	
	/*
	 * the nearest package known from location cur
	 */
	Location Find_Nearest_Package_Known(Location cur){
		KnownPackage nearest = null;
		int min = Integer.MAX_VALUE;
		for(KnownPackage kp : pkg_info){
			int d = dist(new Location(kp.getX(), kp.getY()), cur);
			if (d < min){
				min = d;
				nearest = kp;
			}
		}
		if(nearest != null)
			return new Location(nearest.getX(), nearest.getY());
		else
			return null;
	}
	
	/*
	 * using a random number to decide which package to achieve
	 * 
	 */
	Location Find_Any_Package_Known(Location cur){
		Random r = new Random();
		int index = r.nextInt(pkg_info.size());
		return new Location(pkg_info.get(index).getX(), pkg_info.get(index).getY());
	}
	
	int round = 0;

	/*
	 * 
	 * predicate that determines if location l is clear
	 * pkg_dir is the direction where I hold my package, if any
	 * 
	 */
	boolean is_clear(Location l, int pkg_dir){
		if(pkg_dir == -1)
			return in_map_range(new Point(l.getX(), l.getY(), null)) && map[l.getX()][l.getY()] == map_state.Clear;
		else
			return in_map_range(new Point(l.getX(), l.getY(), null)) && map[l.getX()][l.getY()] == map_state.Clear &&
				   in_map_range(new Point(l.getX() + Direction.DELTA_X[pkg_dir], l.getY() + Direction.DELTA_Y[pkg_dir], null)) && 
				   map[l.getX() + Direction.DELTA_X[pkg_dir]][l.getY() + Direction.DELTA_Y[pkg_dir]] == map_state.Clear;
	}
	
	/*
	 * get all the reachable locations from cur
	 * 
	 */
	Queue<Location> get_all_reachable_locations(Location cur, int pkg_dir){
		boolean[][] visited = new boolean[map_size][map_size];
		for (int i = 0; i < map_size; ++i){
			for (int j = 0; j < map_size; ++j){
				if (map[i][j] == map_state.Unknown || map[i][j] == map_state.Package)
					visited[i][j] = true;
				else
					visited[i][j] = false;
			}
		}
		
		Queue<Location> v1 = new LinkedList<Location>();
		Queue<Location> v2 = new LinkedList<Location>();
		v1.add(cur);

		while (!v1.isEmpty()) {
			Location t = v1.remove();

			if(is_clear(t, pkg_dir))
				v2.add(t);
			
			for (int dir = 0; dir <= 3; ++dir) {
				Location p = new Location(t.getX() + Direction.DELTA_X[dir],
						t.getY() + Direction.DELTA_Y[dir]);
				if (in_map_range(new Point(p, null))
						&& visited[p.getX()][p.getY()] == false) {
					v1.add(p);
					visited[p.getX()][p.getY()] = true;
				}
			}
		}

		return v2;
	}
	
	/*
	 * find the reachable location that nearest to a goal or a package's destination
	 * 
	 */
	Location find_reachable_location_nearest_to_goal_or_dest(Location cur, Location dest, int pkg_dir){
		Queue<Location> all = get_all_reachable_locations(cur, pkg_dir);
		
		Location best = null;
		int dist = Integer.MAX_VALUE;
		while(!all.isEmpty()) {
			Location t = all.remove();
			int temp = Math.abs(dest.getX() - t.getX()) + Math.abs(dest.getY() - t.getY());
			if (temp < dist){
				dist = temp;
				best = t;
			}
		}
		
		return best;
	}
	
	/*
	 * which direction is movable, when the agent is walking in direction dir and holding 
	 * a package in direction pkg_dir
	 */
	int movable_direction(int dir, int pkg_dir){
		if (check_movable(dir, pkg_dir) == false) {
			Random r = new Random();
			int s = r.nextInt(4);
			for (int d = (dir + s) % 4; d != (dir + s + 3) % 4; d = (d + 1) % 4){
				if(check_movable(d, pkg_dir) == true) {
					return d;
				}
			}
			return -1;
		} else
			return dir;
	}
	
	//When I can see the delivery point l, check if that location is free to drop package.
	obj_t object_at_deliverable_point(Location l){
		boolean there_is_me = false;
		boolean there_is_other_agent = false;
		boolean there_is_unheld_package = false;
		boolean there_is_my_held_package = false;
		boolean there_is_others_held_package = false;
		
		for(VisibleAgent va : percept.getVisAgents()){
			if(va.getX() == l.getX() && va.getY() == l.getY()){
				if(this.id == va.getId()){
					there_is_me = true;
					break;
				}
				else{
					there_is_other_agent = true;
					break;
				}
			}
		}
		
		if(there_is_me)
			return obj_t.ME;
		if(there_is_other_agent)
			return obj_t.OTHER_AGENT;
		
		for(VisiblePackage vp : percept.getVisPackages()){
			if(vp.getX() == l.getX() && vp.getY() == l.getY()){
				if(vp.isHeld() == false){
					there_is_unheld_package = true;
					break;
				}else if (vp.isHeld() == true && vp.getId() == percept.getHeldPackage().getId()){
					there_is_my_held_package = true;
					break;
				}
				else if (vp.isHeld() == true && vp.getId() != percept.getHeldPackage().getId()){
					there_is_others_held_package = true;
					break;
				}
			}
		}
	
		if(there_is_my_held_package)
			return obj_t.MY_HELD_PACKAGE;
		if(there_is_others_held_package)
			return obj_t.OTHERS_HELD_PACKAGE;
		if(there_is_unheld_package)
			return obj_t.UNHELD_PACKAGE;	
		return null;
	}
	/**
	 * Provide a Percept to the agent. This function is called by the
	 * environment at the beginning of each agent's turn. If the agent has
	 * internal state, this method should also update it.
	 */
	public void see(Percept p) {
		System.out.println("-------------------------");
		System.out.println("-------------------------");
		System.out.println("Round: " + round);
		round++;

		percept = (PacPercept) p;
		percept.printPercept();
		
		// First time looking at something, initialize map and introduce
		// oneself.
		if (map == null) {
			// initialize map
			map_size = percept.getWorldSize();
			map = new map_state[map_size][map_size];
			for (int i = 0; i < map_size; ++i)
				for (int j = 0; j < map_size; ++j)
					map[i][j] = map_state.Unknown;
		}

		// parse the message that I received
		if (parse_messages(percept.getMessages()) == false)
			return;

		// update info according to the agents I see
		for (VisibleAgent va : percept.getVisAgents()) {
			if (va.getId() == id) {
				this.x = va.getX();
				this.y = va.getY();
			}
		}
		// this is my current location
		cur = new Location(this.x, this.y);
		
		// debug message
		System.out.println("I am Agent " + this.id + " at (" + this.x + ","
				+ this.y + ")");

		// identify new packages if any and broadcast to others
		String msg = "";
		for (VisiblePackage vp : percept.getVisPackages()) {
			if (map[vp.getX()][vp.getY()] == map_state.Unknown) { // this is a new package
														// and no one has ever
														// broadcast it.
				
				KnownPackage kp = new KnownPackage(vp);
				pkg_info.add(kp);
				map[kp.getX()][kp.getY()] = map_state.Package;
				msg += "P"
						+ kp.getId()
						+ "X"
						+ kp.getX()
						+ "Y"
						+ kp.getY()
						+ "Z"
						+ kp.getDestX()
						+ "W"
						+ kp.getDestY()
						+ "H"
						+ ((kp.held == true) ? "Y" : "N")
						+ "O"
						+ ((kp.ordered == true) ? "Y" : "N");
			}
		}

		if (msg != "") { // there are some new packages identified that need to
							// be broadcast to others
			// piggyback information to report where you are
			msg += "A" + this.id + "X" + this.x + "Y" + this.y;
			next_act = new Say(msg);
			return;
		}

		
		// update info according to the agents I see
		for (VisibleAgent va : percept.getVisAgents()) {
			// mark what other agents can see as clear
			update_map(new KnownAgent(va));
		}
		
		if (last_act instanceof Dropoff) {
			// does my last drop-off action delivers the package into its
			// destination?
			int index_package_delivered = -1;
			for (int i = 0; i < percept.getVisPackages().length; ++i) {
				if (percept.getVisPackages()[i].getId() == pkg_delivered)
					index_package_delivered = i;
			}

			if (index_package_delivered >= 0) { // I did find the package,
												// i.e., the package not has been
												// successfully delivered.
				if(percept.getHeldPackage() != null){	//I am still holding the package
					state = state_t.CARRY;
				} else {
					easiest_level++;
					ordered_package.update_loc(new Location(percept.getVisPackages()[index_package_delivered].getX(), percept.getVisPackages()[index_package_delivered].getY()));
					state = state_t.UNORDER;
				}
				
			}else {
				state = state_t.IDLE;
			}
		}

		if (last_act instanceof Pickup) {
			if (percept.getHeldPackage() != null) {
				map[percept.getHeldPackage().getX()][percept.getHeldPackage().getY()] = map_state.Clear;
				
				msg = "P"
						+ percept.getHeldPackage().getId()
						+ "X"
						+ percept.getHeldPackage().getX()
						+ "Y"
						+ percept.getHeldPackage().getY()
						+ "Z"
						+ percept.getHeldPackage().getDestX()
						+ "W"
						+ percept.getHeldPackage().getDestY()
						+ "HY"
						+ "OY";

				msg += "A" + this.id + "X" + this.x + "Y" + this.y; // tell
																	// others
																	// who I am
																	// and where
																	// I am.
				plan.clear();
				state = state_t.CARRY;
				next_act = new Say(msg);
				return;
			} else {
				plan.clear();
				state = state_t.UNORDER;
				easiest_level++;
			}
		}

		while (true) {
			if(state == state_t.ORDER){
				state = state_t.INTENT;
				
				ordered_package = get_package_at_loc(goal);
				pkg_info.remove(ordered_package);
				
				msg = "P"
						+ ordered_package.getId()
						+ "X"
						+ ordered_package.getX()
						+ "Y"
						+ ordered_package.getY()
						+ "Z"
						+ ordered_package.getDestX()
						+ "W"
						+ ordered_package.getDestY()
						+ "HN"
						+ "OY";

				msg += "A" + this.id + "X" + this.x + "Y" + this.y; // tell
																	// others
																	// who I am
																	// and where
																	// I am.
				next_act = new Say(msg);
				return;
			}
			
			if(state == state_t.UNORDER){
				
				pkg_info.add(ordered_package);
				map[ordered_package.getX()][ordered_package.getY()] = map_state.Package;
				msg = "P"
						+ ordered_package.getId()
						+ "X"
						+ ordered_package.getX()
						+ "Y"
						+ ordered_package.getY()
						+ "Z"
						+ ordered_package.getDestX()
						+ "W"
						+ ordered_package.getDestY()
						+ "HN" 
						+ "ON";

				msg += "A" + this.id + "X" + this.x + "Y" + this.y; // tell
																	// others
																	// who I am
																	// and where
																	// I am.
				ordered_package = null;
				next_act = new Say(msg);
				state = state_t.IDLE;
				return;
			}
			
			// if I do not have a intention, I will explore the world until the
			// time I am assigned intention.
			if (state == state_t.IDLE) {
				plan.clear();
				Location l1 = null, l2 = null;
				l1 = Find_N_Easiest_Package_Known(cur, easiest_level);
				if(easiest_level > 1) easiest_level = 1;
				
				l2 = Find_Nearest_Unknown(cur);
				
				if(l1 == null && l2 == null) {
					next_act = new Move(Integer.parseInt(id)%4);
					state = state_t.IDLE;
					return;
				}
				
				if(l1 == null) {
					goal = l2;
					state = state_t.EXPLORE;
					continue;
				}
				
				if(l2 == null) {
					goal = l1;
					state = state_t.ORDER;
					continue;
				}
				
				if(dist(cur, l1) <= dist(cur, l2)){
					goal = l1;
					state = state_t.ORDER;
					continue;
				}
				else{
					goal = l2;
					state = state_t.EXPLORE;
					continue;
				}
			}
			
			
			if (state == state_t.INTENT || state == state_t.EXPLORE) {
				int dira = get_direction(cur, goal);
				if (dira != -1) { // I am close to the goal
					if(state == state_t.INTENT){
						state = state_t.PICKUP;
						continue;
					}
					if(state == state_t.EXPLORE){
						state = state_t.IDLE;
						continue;
					}
				}
				
				ArrayList<Integer> path = null;
				if (plan.size() <= 0) { // generate a plan	
					Random r = new Random();
					int s = r.nextInt(4);
					for (int d = (Integer.parseInt(id) + s)% 4; d != (Integer.parseInt(id) + s + 3) % 4; d = (d + 1) % 4) {
						Point t = new Point(goal.getX()
								+ Direction.DELTA_X[d], goal.getY()
								+ Direction.DELTA_Y[d], null);
						path = compute_path(t, -1);
						if (path != null && path.size() > 0) {
							break;
						}
					}
					
					if (path == null || path.size() <= 0) {
						Location t = find_reachable_location_nearest_to_goal_or_dest(cur, goal, -1);
						path = compute_path(new Point(t, null), -1);
					}
					
					if (path == null || path.size() <= 0) {
						if(state == state_t.INTENT)
								state = state_t.UNORDER;
						else
								state = state_t.IDLE;
						continue;
					}
					
					plan = path;
				}

				// before I continue to go as my plan indicate, check the
				// current instant situation to see
				
				// if there is any moving obstacles(i.e., agents carrying or not
				// carrying a package) in front of me
				// so that I don't collide with them.
				// If there is something in front of me, I just wait and idle.
				Integer go = plan.get(0);

				
				/**********************Handling Collision***********************************/		
				int movable = movable_direction(go, -1);
				
				if(movable != go && movable >= 0){	//the original direction does not work fine, and movable is the next walkable direction
						if (state == state_t.INTENT)
							state = state_t.UNORDER;
						else if (state == state_t.EXPLORE)
							state = state_t.IDLE;

						// update location info
						move_history.add(movable);
						next_act = new Move(movable);
						return;
				} 
				
				if (movable != go && movable == -1){ // all four adjacent blocks are not movable to.
						if (state == state_t.INTENT)
							state = state_t.UNORDER;
						else if (state == state_t.EXPLORE)
							state = state_t.IDLE;
						continue;
				}

				/***************************END Handling Collision*******************************************/
				
				
				plan.remove(0);
				move_history.add(go);
				next_act = new Move(go);
				return;
			}

			if (state == state_t.PICKUP) {
				int dira = get_direction(new Location(this.x, this.y), goal);
				next_act = new Pickup(dira);
				return;
			}

			if (state == state_t.DROPOFF) {
				VisiblePackage pkg_held = percept.getHeldPackage();
				
				if (pkg_held == null) {
					state = state_t.UNORDER;
					easiest_level++;
					continue;
				} else {
					int pkg_dir = get_direction(cur, new Location(pkg_held.getX(),
							pkg_held.getY()));
					
					int dira = get_direction(cur, new Location(pkg_held.getDestX(), pkg_held.getDestY()));
					if(dira < 0){
						pkg_delivered = pkg_held.getId();
						next_act = new Dropoff(dira);
						easiest_level++;
						return;
					}else{	//I am at the delivery point
						obj_t obj = object_at_deliverable_point(new Location(pkg_held.getDestX(), pkg_held.getDestY()));
						if(obj == obj_t.OTHER_AGENT || obj == obj_t.OTHERS_HELD_PACKAGE){
							Random r = new Random();
							next_act = new Move(r.nextInt(4));
							return;
						}else if (obj == obj_t.UNHELD_PACKAGE){
							pkg_delivered = pkg_held.getId();
							next_act = new Dropoff((dira + 2)%4);	//place the package far away from the potential delivery point
							return;
						}else if (obj == obj_t.MY_HELD_PACKAGE || obj == null){
							pkg_delivered = pkg_held.getId();
							next_act = new Dropoff(dira);
							return;
						}else if (obj == obj_t.ME){
							Random r = new Random();
							next_act = new Move(r.nextInt(4));
							return;
						}
					}	
				}
			}
			
			// I am holding some package, so I need to deliver it to its
			// destination no matter what
			if (state == state_t.CARRY) {
				VisiblePackage pkg_held = percept.getHeldPackage();
				System.out.println("agent" + id + " is carrying pkg_held: "
						+ pkg_held);

				int pkg_dir = get_direction(cur, new Location(pkg_held.getX(),
						pkg_held.getY()));

				// check to see if the delivery point is clear (having a static
				// package that is not in my hand)

				// check to see if there is some other agent in the delivery
				// check to see if I am ready to drop the package.
				dest = new Location(pkg_held.getDestX(), pkg_held.getDestY());
				
				int dira = get_direction(cur, dest);
				if (dira != -1) { // I am close to the delivery position
					state = state_t.DROPOFF;
					continue;
				}

				ArrayList<Integer> path = null;
				if (plan.size() <= 0) { // generate a plan
					Random r = new Random();
					int s = r.nextInt(4);
					for (int d = (Integer.parseInt(id) + s)% 4; d != (Integer.parseInt(id) + s + 3) % 4; d = (d + 1) % 4) {
						path = compute_path(new Point(pkg_held.getDestX() + Direction.DELTA_X[d], 
													  pkg_held.getDestY() + Direction.DELTA_Y[d], 
													  null), pkg_dir);
						if (path != null && path.size() > 0) {
							break;
						}
					}
					
					if (path == null || path.size() <= 0) {
						Location t = find_reachable_location_nearest_to_goal_or_dest(cur, dest, pkg_dir);
						path = compute_path(new Point(t, null), pkg_dir);
					}
					
					if (path == null || path.size() <= 0) {
						state = state_t.DROPOFF;
						continue;
					}

					plan = path;
				}

				// before I continue to go as my plan indicate, check the
				// current instant situation to see
				// if there is any moving obstacles(i.e., agents carrying or not
				// carrying a package) in front of me
				// so that I don't collide with them.
				// If there is something in front of me, I just wait and idle.
				Integer go = plan.get(0);

				/**********************Handling Collision***********************************/		

				int movable = movable_direction(go, pkg_dir);
				
				if(movable != go && movable >= 0){	//the original direction does not work fine, and movable is the next walkable direction
						// update location info
						plan.clear();
						move_history.add(movable);
						next_act = new Move(movable);
						return;
				} 
				
				if (movable != go && movable == -1){ // all four adjacent blocks are not movable to.
						state = state_t.DROPOFF;
						continue;
				}
				
				
				/***************************END Handling Collision*******************************************/

				plan.remove(0);
				move_history.add(go);
				next_act = new Move(go);
				return;
			}// end carry state
		} // while
	}

	/**
	 * Have the agent select its next action to perform. Implements the action0:
	 * Per -> Ac function or the action: I -> Ac function, depending on whether
	 * or not the agent has internal state. Note, Per (or I) is not passed as a
	 * parameter, because we assume this information is recorded in an instance
	 * variable by the see() method.
	 */
	public Action selectAction() {
		last_act = next_act;
		return next_act;
	}

	/**
	 * Return a unique string that identifies the agent. This is particularly
	 * useful in multi-agent environments.
	 */
	public String getId() {
		return id;
	}

	public void printInfo() {
		System.out.println("Agents Known:");
		for (int i = 0; i < agt_info.size(); i++)
			System.out.println(agt_info.get(i));
		System.out.println("Package Known:");
		for (int i = 0; i < pkg_info.size(); i++)
			System.out.println(pkg_info.get(i));
		System.out.println("Map as far as I known:");
		for (int j = 0; j < map_size; ++j) {
			for (int i = 0; i < map_size; ++i) {
				if (map[i][j] == map_state.Unknown)
					System.out.print("*");
				else if (map[i][j] == map_state.Package)
					System.out.print("#");
				else if (map[i][j] == map_state.Clear)
					System.out.print("_");
				else
					assert (false);
			}
			System.out.println();
		}
	}

	public boolean in_map_range(Point p) {
		return p.getX() >= 0 && p.getX() < map_size && p.getY() >= 0
				&& p.getY() < map_size;
	}

}
